# -*- encoding: utf-8 -*-
# 测试环境: Python 3.6 版本
import _thread
import json
import socket
import time
import sdk

color_dict = {"yellow": [255, 255, 0],
              "blue": [0, 0, 255],
              "green": [0, 255, 0],
              "red": [255, 0, 0]}


def get_ip():
    ip_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定 IP 广播端口
    ip_sock.bind(('0.0.0.0', 40926))
    return (ip_sock.recvfrom(1024))[1][0]


def check_state_to_color(check_state):
    color = color_dict["yellow"]
    if check_state == "TRACE_COLLECT":
        color = color_dict["yellow"]
    elif check_state == "INV_GENERATING" or check_state == "INV_GENERATED":
        color = color_dict["blue"]
    elif check_state == "INV_NOT_VIOLATED":
        color = color_dict["green"]
    elif check_state == "INV_VIOLATED":
        color = color_dict["red"]
    return color


def set_color(rbs, name, color):
    sdk.send_command(rbs, sdk.set_led("bottom_" + name,
                                      color[0], color[1], color[2],
                                      "solid"))


def send_msg(rbs):
    sk = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    addr = ('127.0.0.1', 8080)
    while True:
        # time.sleep(0.05)
        front = float(sdk.send_command(rbs, sdk.get_sensor_value(1)))
        right = float(sdk.send_command(rbs, sdk.get_sensor_value(2)))
        back = float(sdk.send_command(rbs, sdk.get_sensor_value(3)))
        left = float(sdk.send_command(rbs, sdk.get_sensor_value(4)))
        sensor = {"front": front, "right": right, "back": back, "left": left}
        sensor_json = json.dumps(sensor)
        # print(sensor_json)
        sk.sendto(sensor_json.encode(), addr)


def recv_msg(rbs):
    sk = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定 IP 和端口号
    sk.bind(('127.0.0.1', 8081))
    while True:
        # 接收数据报
        msg, addr = sk.recvfrom(1024)
        # 打印
        kv = json.loads(msg.decode())
        # print(kv)
        if kv["channel"] == "actor":
            actor = json.loads(kv["message"])
            sdk.send_command(rbs, sdk.set_speed(False, actor["xSpeed"], actor["ySpeed"], actor["zSpeed"]))
        elif kv["channel"] == "check":
            check_info = json.loads(kv["message"])
            color = check_state_to_color(check_info["checkState"])
            set_color(rbs, check_info["name"], color)


def main():
    # 路由模式
    address = (get_ip(), int(40923))
    # 直连模式
    # address = ("192.168.2.1", int(40923))
    # 与机器人控制命令端口建立 TCP 连接
    rbs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    print("Connecting...")
    rbs.connect(address)
    print("Connected!")

    if sdk.send_command(rbs, sdk.command()) == "ok":
        print("init ok")
    if sdk.send_command(rbs, sdk.set_sensor_measure(True)) == "ok":
        print("sensor measure on")

    try:
        _thread.start_new_thread(send_msg, (rbs,))
        _thread.start_new_thread(recv_msg, (rbs,))
    finally:
        pass

    while True:
        time.sleep(1)
        print(sdk.send_command(rbs, sdk.get_position()))

    # 关闭端口连接
    rbs.shutdown(socket.SHUT_WR)
    rbs.close()


if __name__ == '__main__':
    main()
